// Copyright FYT 邹承甫 2022

#ifndef POST_PROCESSOR__KALMAN_FILTER_HPP_
#define POST_PROCESSOR__KALMAN_FILTER_HPP_

#include <memory>

#include <Eigen/Core>

namespace post_processor {

class KalmanFilter {
public:
  using Ptr = std::unique_ptr<KalmanFilter>;
  KalmanFilter(const Eigen::MatrixXd& F, const Eigen::MatrixXd& P,
               const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R,
               const Eigen::MatrixXd& H);
  void initialize(const Eigen::VectorXd &x0);
  Eigen::VectorXd update(const Eigen::VectorXd &measurement, const double &dt);
  Eigen::VectorXd predict(const double &dt);

private:
  bool chiSquareTest(const Eigen::MatrixXd &residual,
                     const Eigen::MatrixXd &covariance);
  bool normTest(const Eigen::MatrixXd &residual);

  bool chi_square_flag_;

  Eigen::MatrixXd F_;
  Eigen::MatrixXd P_;
  Eigen::MatrixXd Q_;
  Eigen::MatrixXd R_;
  Eigen::MatrixXd H_;
  Eigen::MatrixXd P_predict_;
  Eigen::MatrixXd P_init_;
  Eigen::VectorXd x_;
  Eigen::VectorXd x_predict_;
};

} // namespace post_processor

#endif // POST_PROCESSOR__KALMAN_FILTER_HPP_